function plot_graph_slam(true_state, pose_graph, trajectory, obstacles, time)
% Graph-SLAM可视化（改进版）

    clf; hold on; grid on; axis equal;
    
    % 绘制障碍物
    for i = 1:size(obstacles, 1)
        obs = obstacles(i, :);
        rectangle('Position', [obs(1), obs(2), obs(3)-obs(1), obs(4)-obs(2)], ...
                  'FaceColor', [0.7 0.7 0.7], 'EdgeColor', 'k');
    end
    
    % 用于图例的句柄
    h_legend = [];
    legend_labels = {};
    
    % 创建障碍物的图例代理（使用patch）
    h = patch(NaN, NaN, [0.7 0.7 0.7], 'EdgeColor', 'k');
    h_legend = [h_legend, h];
    legend_labels{end+1} = '障碍物';
    
    % 轨迹
    if size(trajectory, 2) > 0
        h = plot(trajectory(1, :), trajectory(2, :), 'b-', 'LineWidth', 1.5);
        h_legend = [h_legend, h];
        legend_labels{end+1} = '轨迹';
    end
    
    % 当前真实位姿
    if ~isempty(true_state)
        draw_robot(true_state(1), true_state(2), true_state(3), 0.8, 'g');
    end
    
    % 估计的路标（从图中提取）
    if pose_graph.landmark_map.Count > 0
        lm_keys = cell2mat(keys(pose_graph.landmark_map));
        n_lm = length(lm_keys);
        est_lm = zeros(n_lm, 2);
        
        for i = 1:n_lm
            lm_id = lm_keys(i);
            lm_idx = double(pose_graph.landmark_map(lm_id));  % 转换为double
            % 使用estimate_landmark_position估计路标位置
            lm_pos = estimate_landmark_position(pose_graph, lm_idx, size(pose_graph.poses, 2));
            est_lm(i, :) = lm_pos';
        end
        
        if n_lm > 0
            h = plot(est_lm(:, 1), est_lm(:, 2), 'ro', 'MarkerSize', 6, 'LineWidth', 1.5);
            h_legend = [h_legend, h];
            legend_labels{end+1} = '路标';
        end
    end
    
    % 创建图例
    if ~isempty(h_legend)
        legend(h_legend, legend_labels, 'Location', 'best');
    end
    xlabel('X(m)'); ylabel('Y(m)');
    title(sprintf('Graph-SLAM 数据收集 (t=%.1fs, 路标:%d)', time, pose_graph.landmark_map.Count));
    xlim([-2, 52]); ylim([-2, 52]);
    hold off;
end

function draw_robot(x, y, theta, size, color)
    robot_points = size * [0.5, 0; -0.5, 0.3; -0.5, -0.3; 0.5, 0];
    R = [cos(theta), -sin(theta); sin(theta), cos(theta)];
    robot_points = (R * robot_points')';
    robot_points(:, 1) = robot_points(:, 1) + x;
    robot_points(:, 2) = robot_points(:, 2) + y;
    plot(robot_points(:, 1), robot_points(:, 2), color, 'LineWidth', 2);
end

